/*
* Copyright (C) Microsoft. All rights reserved.  
*
* Licensed under the Apache License, Version 2.0 (the "License"); you may not 
* use this file except in compliance with the License.  You may obtain a copy 
* of the License at http://www.apache.org/licenses/LICENSE-2.0  
* 
* THIS CODE IS PROVIDED *AS IS* BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
* KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT LIMITATION ANY IMPLIED 
* WARRANTIES OR CONDITIONS OF TITLE, FITNESS FOR A PARTICULAR PURPOSE, 
* MERCHANTABLITY OR NON-INFRINGEMENT. 
*
* See the Apache License, Version 2.0 for specific language governing 
* permissions and limitations under the License.
*
*/
#pragma once

#include <baja/common.hpp>
#include <baja/math.hpp>

namespace baja
{

// this class implements a trapezoidal velocity interpolation
// what this means is the velocity function has the shape of a trapezoid, and hence the space function looks like an "s"
// this ultimately achieves smoother motion around the edges of the interpolation
// there's a period of acceleration and a period of deceleration, both determined as a percentage of the total duration (parameters p1 and p2)
class tv_interpolation
{
public:
    tv_interpolation() {}

    tv_interpolation(
        float32 t0,
        float32 t3,
        float32 a,
        float32 b,
        float32 p1, // (0..1) percentage of time to accelerate
        float32 p2 // (0..1) percentage of time to decelerate
        )
    {
		this->set(t0, t3, a, b, p1, p2);
    }

    float32 apply(const float32 t)
    {
        if (t <= _t0)
        {
            return _a;
        }
        else if (t <= _t1)
        {
            return _a1 * (t - _t0) * (t - _t0) + _a;
        }
        else if (t <= _t2)
        {
            return  _v * (t - _t1) + _x1;
        }
        else if (t <= _t3)
        {
            return _a2 * (t - _t2) * (t - _t2) + _v * (t - _t2) + _x2;
        }
        else
        {
            return _b;
        }
    }

    float32 a() { return _a; }
    float32 b() { return _b; }
	float32 t0() { return _t0; }
	float32 t3() { return _t3; }

	void set(
        float32 t0,
        float32 t3,
        float32 a,
        float32 b,
        float32 p1, // (0..1) percentage of time to accelerate
        float32 p2 // (0..1) percentage of time to decelerate
        ) 
	{
		_t0 = t0;
		_t3 = t3;
		_a = a;
		_b = b;

		// note
        // I deducted this by doing the math, basically you have three ecuations for space over time (for the three periods of time, acceleration, constant velocity and deceleration)
        // then you have another three equations for the derivatives (speed over time)
        // you have a set of fixed variables, like the time periods and the initial and final velocities
        // and a set of constraints, like the fact that speed and position at the joints of the functions must be equal
        // then resolve that system of ecuations and you get this
        _t1 = ((_t3 - _t0) * p1) + _t0;
        _t2 = _t3 - ((_t3 - _t0) * p2);

        float32 temp1 = -1.f * (_t3 - _t2) * (_t3 - _t2) - 2.f * (_t3 - _t2) * (_t2 - _t1) - (_t3 - _t2) * (_t1 - _t0);
        _a2 = (b - a) / temp1;
        _a1 = (-1.f * _a2 * (_t3 - _t2)) / (_t1 - _t0);

        _v = 2.f * _a1 * (_t1 - _t0);

        _x1 = _a1 * (_t1 - _t0) * (_t1 - _t0) + _a;
        _x2 = _v * (_t2 - _t1) + _x1;
	}

private:
    float32 _t0, _t1, _t2, _t3;
    float32 _a, _b;
    float32 _a1, _a2, _v;
    float32 _x1, _x2;
};

template <class Tv>
class tv_interpolation_2d
{
public:
    tv_interpolation_2d() {}

    tv_interpolation_2d(
        float32 t0,
        float32 t3,
        Tv a,
        Tv b,
        float32 p1,
        float32 p2
        )
    {
        _tvix = tv_interpolation(t0, t3, a.x, b.x, p1, p2);
        _tviy = tv_interpolation(t0, t3, a.y, b.y, p1, p2);
    }

    Tv apply(const float32 t)
    {
        Tv res;
        res.x = _tvix.apply(t);
        res.y = _tviy.apply(t);
        return res;
    }

    Tv a() { return _a; }
    Tv b() { return _b; }
	float32 t0() { return _tvix.t0(); }
	float32 t3() { return _tvix.t3(); }

	void set(
        float32 t0,
        float32 t3,
        Tv a,
        Tv b,
        float32 p1,
        float32 p2
		)
	{
		_tvix.set(t0, t3, a.x, b.x, p1, p2);
		_tviy.set(t0, t3, a.y, b.y, p1, p2); 
	}

private:
    tv_interpolation _tvix;
    tv_interpolation _tviy;
};

template <class Tv>
class tv_interpolation_3d
{
public:
    tv_interpolation_3d() {}

    tv_interpolation_3d(
        float32 t0,
        float32 t3,
        Tv a,
        Tv b,
        float32 p1,
        float32 p2
        )
    {
        _tvix = tv_interpolation(t0, t3, a.x, b.x, p1, p2);
        _tviy = tv_interpolation(t0, t3, a.y, b.y, p1, p2);
        _tviz = tv_interpolation(t0, t3, a.z, b.z, p1, p2);
    }

	void set(
        float32 t0,
        float32 t3,
        Tv a,
        Tv b,
        float32 p1,
        float32 p2
		)
	{
		_tvix.set(t0, t3, a.x, b.x, p1, p2);
		_tviy.set(t0, t3, a.y, b.y, p1, p2); 
		_tviz.set(t0, t3, a.z, b.z, p1, p2);
	}

    Tv a() { return _a; }
    Tv b() { return _b; }
	float32 t0() { return _tvix.t0(); }
	float32 t3() { return _tvix.t3(); }

	Tv apply(const float32 t)
    {
        Tv res;
        res.x = _tvix.apply(t);
        res.y = _tviy.apply(t);
        res.z = _tviz.apply(t);
        return res;
    }

private:
    tv_interpolation _tvix;
    tv_interpolation _tviy;
    tv_interpolation _tviz;
};

};